#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

static void
test_single_voxel_patch()
{
	Tensor<float, 5> tensor(4, 2, 3, 5, 7);
	tensor.setRandom();
	Tensor<float, 5, RowMajor> tensor_row_major = tensor.swap_layout();

	Tensor<float, 6> single_voxel_patch;
	single_voxel_patch = tensor.extract_volume_patches(1, 1, 1);
	VERIFY_IS_EQUAL(single_voxel_patch.dimension(0), 4);
	VERIFY_IS_EQUAL(single_voxel_patch.dimension(1), 1);
	VERIFY_IS_EQUAL(single_voxel_patch.dimension(2), 1);
	VERIFY_IS_EQUAL(single_voxel_patch.dimension(3), 1);
	VERIFY_IS_EQUAL(single_voxel_patch.dimension(4), 2 * 3 * 5);
	VERIFY_IS_EQUAL(single_voxel_patch.dimension(5), 7);

	Tensor<float, 6, RowMajor> single_voxel_patch_row_major;
	single_voxel_patch_row_major = tensor_row_major.extract_volume_patches(1, 1, 1);
	VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(0), 7);
	VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(1), 2 * 3 * 5);
	VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(2), 1);
	VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(3), 1);
	VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(4), 1);
	VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(5), 4);

	for (int i = 0; i < tensor.size(); ++i) {
		VERIFY_IS_EQUAL(tensor.data()[i], single_voxel_patch.data()[i]);
		VERIFY_IS_EQUAL(tensor_row_major.data()[i], single_voxel_patch_row_major.data()[i]);
		VERIFY_IS_EQUAL(tensor.data()[i], tensor_row_major.data()[i]);
	}
}

static void
test_entire_volume_patch()
{
	const int depth = 4;
	const int patch_z = 2;
	const int patch_y = 3;
	const int patch_x = 5;
	const int batch = 7;

	Tensor<float, 5> tensor(depth, patch_z, patch_y, patch_x, batch);
	tensor.setRandom();
	Tensor<float, 5, RowMajor> tensor_row_major = tensor.swap_layout();

	Tensor<float, 6> entire_volume_patch;
	entire_volume_patch = tensor.extract_volume_patches(patch_z, patch_y, patch_x);
	VERIFY_IS_EQUAL(entire_volume_patch.dimension(0), depth);
	VERIFY_IS_EQUAL(entire_volume_patch.dimension(1), patch_z);
	VERIFY_IS_EQUAL(entire_volume_patch.dimension(2), patch_y);
	VERIFY_IS_EQUAL(entire_volume_patch.dimension(3), patch_x);
	VERIFY_IS_EQUAL(entire_volume_patch.dimension(4), patch_z * patch_y * patch_x);
	VERIFY_IS_EQUAL(entire_volume_patch.dimension(5), batch);

	Tensor<float, 6, RowMajor> entire_volume_patch_row_major;
	entire_volume_patch_row_major = tensor_row_major.extract_volume_patches(patch_z, patch_y, patch_x);
	VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(0), batch);
	VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(1), patch_z * patch_y * patch_x);
	VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(2), patch_x);
	VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(3), patch_y);
	VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(4), patch_z);
	VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(5), depth);

	const int dz = patch_z - 1;
	const int dy = patch_y - 1;
	const int dx = patch_x - 1;

	const int forward_pad_z = dz / 2;
	const int forward_pad_y = dy / 2;
	const int forward_pad_x = dx / 2;

	for (int pz = 0; pz < patch_z; pz++) {
		for (int py = 0; py < patch_y; py++) {
			for (int px = 0; px < patch_x; px++) {
				const int patchId = pz + patch_z * (py + px * patch_y);
				for (int z = 0; z < patch_z; z++) {
					for (int y = 0; y < patch_y; y++) {
						for (int x = 0; x < patch_x; x++) {
							for (int b = 0; b < batch; b++) {
								for (int d = 0; d < depth; d++) {
									float expected = 0.0f;
									float expected_row_major = 0.0f;
									const int eff_z = z - forward_pad_z + pz;
									const int eff_y = y - forward_pad_y + py;
									const int eff_x = x - forward_pad_x + px;
									if (eff_z >= 0 && eff_y >= 0 && eff_x >= 0 && eff_z < patch_z && eff_y < patch_y &&
										eff_x < patch_x) {
										expected = tensor(d, eff_z, eff_y, eff_x, b);
										expected_row_major = tensor_row_major(b, eff_x, eff_y, eff_z, d);
									}
									VERIFY_IS_EQUAL(entire_volume_patch(d, z, y, x, patchId, b), expected);
									VERIFY_IS_EQUAL(entire_volume_patch_row_major(b, patchId, x, y, z, d),
													expected_row_major);
								}
							}
						}
					}
				}
			}
		}
	}
}

EIGEN_DECLARE_TEST(cxx11_tensor_volume_patch)
{
	CALL_SUBTEST(test_single_voxel_patch());
	CALL_SUBTEST(test_entire_volume_patch());
}
